/* KFApp c++ hearder file KFApp.h */
/*
	By     : Yan Gongmin @ NWPU
	Date   : 2017-04-29
	From   : College of Automation,
			 Northwestern Polytechnical University,
			 Xi'an 710072, China
*/

#ifndef _KFAPP_H
#define _KFAPP_H

#include "PSINS.h"

// Z和R的维数

#define GPSVN	0
#define GPSPOS	3
#define ZUPT	6		// 静止零速修正速度
#define ZUPTYAW 9		// 静止零速修正航向
#define CARMC	10		// Car Moving Constraint
#define ODOVN   13   //里程计速度[0;odovn;0]
#define GPSYAW	16  //GPS航向角
#ifdef __cplusplus
extern "C" {
#endif
// 车载航姿仪类
class CCarAHRS : public CTDKF {
public:
    BOOL levelAlignOK, yawAlignOK, initPosOK, opt_zuptA;
    BOOL odo_tag, holo_tag;
    double measGPSYaw;
    CVect3 measGPSVn, measGPSPos, measINSvn, measCMvb, measMag, measODOVN;
    CAligni0 align;
    CSINS sins;

    CCarAHRS();

    void SetMeasGPSVn(CVect3 &vnGPS);

    void SetMeasGPSPos(CVect3 &posGPS);
    void SetMeasGPSPos(CVect3 &posGPS,const double dt);
    void SetMeasGPSYaw(double gpsYaw);

    void SetMeasZUPT(void);

    void SetMeasMC(void);

    void SetMeasHOLO(void);

    void SetMeasODOVN(CVect3 &odoVN);

    void SetMeasMag(CVect3 &mag);

    virtual void MeasRearrange(CSINS &sins);

    void SetPk(CVect3 att_var, CVect3 vel_var, CVect3 pos_var, double bg_var, double ba_var, double kd_var);

    void SetQt(double arw, double vrw, double prw, double bg_var, double bg_Tcor, double ba_var, double ba_Tcor,
               double kd_var, double kd_Tcor);

    void SetRk_GPSVN(CVect3 std_vn);

    void SetRk_GPSPOS(CVect3 std_pos);

    void SetRk_ZUPT(CVect3 var_zupt, double var_zuptA);

    void SetRk_ODO(CVect3 var_odo);

    void SetRk_CARMC(double sts);

    void SetRk_HOLO(double var_holo);

    void Align_Static(CVect3 &pos0, CVect3 &v0, CVect3 &att0);

    void Align_InMotion(CVect3 &wm, CVect3 &vm, double tk, double ts);

    int Update(CVect3 &wm1, CVect3 &vm1, CVect3 &wm, CVect3 &vm, double ts);

    ~CCarAHRS();
};
#ifdef __cplusplus
}
#endif
#endif

